﻿//includes
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

//napespace
namespace PayloadTracker.APRS.SoundModem
{

    //delegate for when a bit is ready for processing.
    public delegate void FSKBitReadyDelegate(bool bit);
    /// <summary>
    /// Frequency Shift Keying
    /// </summary>
    public class FSK
    {

        //structure that defines AFSK modem type
        public struct ModemDeffinition
        {

            public int FreqSpace; //Tone for 0
            public int FreqMark; //Tone for 1
            public int BaudRate; //baud rate of protocol.
            public int SampleRate; //how many samples per second are we demodulating.

            //constructor
            public ModemDeffinition(int FreqSpace, int FreqMark, int BaudRate, int SampleRate)
            {
                this.FreqSpace = FreqSpace;
                this.FreqMark = FreqMark;
                this.BaudRate = BaudRate;
                this.SampleRate = SampleRate;
            }
        }

        //soundcard device
        public struct CaptureDevice
        {
            public uint ID; // windows ID
            public string Name; //windows Name
            public override string ToString()
            {
                return ID.ToString() +": " + Name;
            }
            public CaptureDevice(uint ID, string Name)
            {
                this.ID = ID;
                this.Name = Name;
            }
        }
        //Get audio sources available in the OS.
        public static List<CaptureDevice> GetAvailableDevices()
        {
            var ret = new List<CaptureDevice>();

            int devs = WaveLib.WaveNative.waveInGetNumDevs();

            for(uint i=0;i<=devs;i++)
            {
                WaveLib.WaveNative.WaveInCaps caps = new WaveLib.WaveNative.WaveInCaps();
                WaveLib.WaveNative.waveInGetDevCaps(i, caps, System.Runtime.InteropServices.Marshal.SizeOf(caps));

                CaptureDevice cd = new CaptureDevice();
                cd.Name = caps.szName;
                cd.ID = i;
                
                ret.Add(cd);
            }

            return ret;
        }

        //static deffinition for BELL202
        public static readonly ModemDeffinition Bell202 = new ModemDeffinition(2200, 1200, 1200, 44100);//22050
        //event that people can subscribe to to recieve notifications.
        public event FSKBitReadyDelegate FSKBitReady;

        ModemDeffinition m_Modem;

        WaveLib.WaveInRecorder m_Recorder;
        int m_Device;
        byte[] m_RecordBuffer;


        double[,] m_Correlates;
        int m_CorrelatesCount;

        double[] m_RingBuffer;
        int m_RingBufferIndex;

        double m_CellAdjustment;

        bool m_CurrentBit, m_PreviousProcessedBit;

        int m_QuietConsecutive;

        uint m_SHREG;
        uint m_PHASE;
        uint m_LASTS;

        Filters.LowPass m_LowPass;
        Filters.HighPass m_HighPass;


        //construcotr
        public FSK(ModemDeffinition modem, int CaptureDevice)
        {
            Initialize(modem, CaptureDevice, false);
        }
        //destructor
        ~FSK()
        {
            if (m_Recorder != null)
                m_Recorder.Dispose();
        }
        //initialize FSK
        private void Initialize(ModemDeffinition modem, int DeviceID, bool downsample)
        {
            //setup filters
            m_LowPass = new Filters.LowPass(Math.Max(modem.FreqMark, modem.FreqSpace),modem.SampleRate);
            m_HighPass = new Filters.HighPass(Math.Min(modem.FreqMark, modem.FreqSpace), modem.SampleRate);
            m_Modem = modem;
            m_Device = DeviceID;

            //reset states
            m_SHREG = 0;
            m_PHASE = 0;
            m_LASTS = 0;
            m_PreviousProcessedBit = m_CurrentBit = true;




            //Calculate correlate size (aka the number of samples needed for the smallest wave)
            m_CorrelatesCount = m_Modem.SampleRate / m_Modem.FreqMark;
            m_Correlates = new double[4, m_CorrelatesCount];


            double fmark = 0;
            double fspace = 0;
            //Fill the table
            for (int i = 0; i < m_CorrelatesCount; i++)
            {
                m_Correlates[0, i] = Math.Sin(fmark);
                m_Correlates[1, i] = Math.Cos(fmark);
                m_Correlates[2, i] = Math.Sin(fspace);
                m_Correlates[3, i] = Math.Cos(fspace);

                //incriment our corolates.
                fmark += 2.0 * Math.PI * (double)m_Modem.FreqMark / (double)m_Modem.SampleRate;
                fspace += 2.0 * Math.PI * (double)m_Modem.FreqSpace / (double)m_Modem.SampleRate;

            }

            //Create the ring buffer
            m_RingBuffer = new double[m_CorrelatesCount];
            for (int i = 0; i < m_RingBuffer.Length; i++)
                m_RingBuffer[i] = 0;

            m_RingBufferIndex = 0;

            m_CellAdjustment = m_Modem.BaudRate / ((double)m_Modem.SampleRate);

            m_QuietConsecutive = 10;
        }
        public void Start()
        {
            //begin recording from audio device
            m_Recorder = new WaveLib.WaveInRecorder(m_Device, new WaveLib.WaveFormat(m_Modem.SampleRate, 16, 1), m_Modem.SampleRate, 3, new WaveLib.BufferDoneEventHandler(OnRecordedData));
        }
        public void Stop()
        {
            //stop recording from audio device
            if(m_Recorder != null)
                m_Recorder.Dispose();
            m_Recorder = null;
        }
        void OnRecordedData(IntPtr data, int size)
        {
            //copy data from the recordered buffer ad pass it to our analyzser

            //adjust array
            if(m_RecordBuffer == null || size > m_RecordBuffer.Length)
                m_RecordBuffer = new byte[size];
            

            //copy
            System.Runtime.InteropServices.Marshal.Copy(data, m_RecordBuffer, 0, size);
            var ms = new System.IO.MemoryStream(m_RecordBuffer);
            var br = new System.IO.BinaryReader(ms);

            //pass it to analyzer
            for (int i = 0; i < size / 2; i++)
            {
                var sample = br.ReadInt16();
                ProcessSample(sample / 32768.0f); //normalize sample between -1 and 1.
            }
        }
        uint PhaseInc()
        {//static bit phase increment.
            var ret = (uint)(0x10000u*(((double)m_Modem.BaudRate)/((double)m_Modem.SampleRate)));
            return ret;
        }

        public void ProcessSample(double[] samples)
        {//overload of sample processor
            foreach(double d in samples)
                ProcessSample(d);
        }
        public void ProcessSample(double sample)
        {

            

            //how loud is this sample?
            if (Math.Abs(sample) < 0.05)
            {
                if (m_QuietConsecutive == 9)
                {//we hit quiet time to reset.
                    Logger.LOG.WriteLine("Quiet period hit. Resetting FSK.");
                    //Reset values to default.

                    m_LASTS = m_PHASE = m_SHREG = 0;
                    m_PreviousProcessedBit = m_CurrentBit = true;
                    m_LowPass.Reset();
                    m_HighPass.Reset();
                    for (int i = 0; i < m_RingBuffer.Length; i++)
                        m_RingBuffer[i] = 0;

                    m_QuietConsecutive = 10;
                    return;
                }
                else if (m_QuietConsecutive == 10)
                    //we are in a quiet period and waiting for a trigger. do nothing.
                    return;
                else
                    m_QuietConsecutive++;

            }
            else
                m_QuietConsecutive = 0;



            //if we get here then we just broke squelch and are processing a smaple.

            //Logger.LOG.WriteLine(sample.ToString());

            Logger.LOG.AudioSample((float)sample);


            //store sample in ring buffer.
            m_RingBuffer[m_RingBufferIndex++] = sample;
            if (m_RingBufferIndex >= m_RingBuffer.Length)
                m_RingBufferIndex = 0;


            //do correlate calculations 
            double[] factors = { 0, 0, 0, 0 };
            for (int i = 0, j = m_RingBufferIndex; i < m_RingBuffer.Length; i++)
            {
                //handle ring buffer.
                if (j >= m_RingBuffer.Length)
                    j = 0;

                var v = m_RingBuffer[j];

                factors[0] += m_Correlates[0, i] * v; //mark q
                factors[1] += m_Correlates[1, i] * v; //mark i
                factors[2] += m_Correlates[2, i] * v; //space q
                factors[3] += m_Correlates[3, i] * v; //space i
                j++;
            }
            //compute the fitness and find out we are biased towards true or false.

            double fitness = (factors[0]*factors[0] + factors[1]*factors[1]) - (factors[2]*factors[2] + factors[3]*factors[3]);

            //adjust registry
            m_SHREG <<= 1;
            //store bit.
            m_SHREG |= (fitness > 0) ? 1u : 0u;

            //See if we are in a bit transition
            if (((m_SHREG ^ (m_SHREG >> 1)) & 1) != 0)
            {
                //Adjust bit phase.
                if (m_PHASE < (0x8000u - (PhaseInc() / 2)))
                    m_PHASE += ((uint)(PhaseInc() / 8)); //foward
                else
                    m_PHASE -= ((uint)(PhaseInc() / 8)); //back
            }
            //if we processed enough for a bit then store it and dispatch event.
            m_PHASE += ((uint)(PhaseInc()));
            if (m_PHASE >= 0x10000u)
            {
                m_PHASE &= 0xffffu;
                m_LASTS <<= 1;
                m_LASTS |= m_SHREG & 1;
                Logger.LOG.Write((m_SHREG & 1) == 1 ? "1" : "0");

                //AX.25 SPECIFIC CODE 
                //Perform NRZI (Non return to zero inverted) encoding.
                //This means that a 0 bit is encoded as a change in tone while a 1 is encoded as NO CHANGE in tone.
                
                m_CurrentBit = ((m_LASTS ^ (m_LASTS >> 1) ^ 1) & 1) == 1;
                ProcessBit(m_CurrentBit);
                
            }
        }
        void ProcessBit(bool bit)
        {
            //Logger.LOG.Write(bit?"1":"0");
            FSKBitReadyDelegate handler = FSKBitReady;
            if(handler != null)
                handler(bit);
        }
    }
}
